// Meena Abdou
// CPEG 467 - ERA MOBI Robot project
// 10/13/2010

/*
 * Drive around by hugging walls.  Should be able to traverse a maze.
 * Hugs the right wall.
 * 
 *
 */

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <fstream>

#include "args.h"

#define FRONT 327
#define RIGHT 110
#define LEFT 546

using namespace PlayerCc;
    
PlayerClient robot(gHostname, gPort);
Position2dProxy pp(&robot, gIndex);
LaserProxy lp(&robot, gIndex);
std::ofstream fout;

void saveData(){
  double x = pp.GetXPos();
  double y = pp.GetYPos();
  double yaw = pp.GetYaw();

  //fout << "X: "; 
  fout.width(13); 
  fout << x; 
  fout << " ";
  //fout << "Y: ";
  fout.width(13);
  fout << y;
  fout << " ";
  //fout << "Yaw: ";
  fout.width(13);
  fout << rtod(yaw);
  fout << " ";
  //fout << "Right: "; 
  fout << lp[RIGHT];
  fout << " ";
  //fout << "Left: ";
  fout << lp[LEFT] << "\n";
}

int main(int argc, char **argv)
{
  parse_args(argc,argv);

  fout.open("OutputData.txt");

  if(!fout.is_open()){
    std::cerr << "Error opening file\n";
    exit(-1);
  }

  fout.setf(std::ios::fixed, std::ios::floatfield);
  fout.precision(8);

  // we throw exceptions on creation if we fail
  try
  {
    pp.SetMotorEnable (true);
    
    double newspeed = 0;
    double newturnrate = 0;
    //double minR = 5.6;
    //double minL = 5.6;

    robot.Read(); //here so GetRange doesn't segfault
      
    // go into read-think-act loop
    for(;;)
    {

      // this blocks until new data comes; 10Hz by default
      robot.Read();

      /* Using only the certain sensors - front, left, right */
      double front = lp[327];
      double frontRight = lp[218];
      double frontLeft = lp[437];
      double right = lp[RIGHT];
      double left = lp[LEFT];
      double yaw = pp.GetYaw();

      if(right <= 0.50){
	newspeed = 0.2;
	newturnrate = 0.375;
      }
      else if(right >= 1.0){
	newspeed = 0.2;
	newturnrate = -0.375;
      }
      else if(right >= 5.58){
	newspeed = 0.0;
	newturnrate = -0.75;
      }
      else if(rtod(yaw) >= 10){
	newspeed = 0.2;
	newturnrate = dtor(-10);
      }
      else if(rtod(yaw) <= -10){
	newspeed = 0.2;
	newturnrate = dtor(10);
      }

      /*
      else if(left <= 0.5){
	newspeed = 0.0;
	newturnrate = -0.375;
      }
      else if(right <= 1.0){
	newspeed = 0.2;
	newturnrate = 0.375;
      }
      else if(left <= 1.0){
	newspeed = 0.2;
	newturnrate = -0.375;
      }
      else if(front <= 1.5){
	newspeed = 0.2;
	newturnrate = 0.375;
      }
      */

      else{
	newspeed = 0.5;
	newturnrate = 0.0;
      }
      
      //printf("Front: %f\nRight: %f\nLeft: %f\n\n", front, right, left);
      pp.SetSpeed(newspeed, newturnrate);
      //getchar();
      saveData();
    }
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
